#include "Octree.h"

int Octree::MAX_LEVEL = 5;
unsigned Octree::MAX_OBJECTS = 5;

#include "../collider/SphereCollider.hpp"
#include "../../game/object/Asteroid.h"

#include "Global.h"

Octree::Octree(int level, glm::vec3 position, float halfSize) : level(level), position(position), halfSize(halfSize) {

}

Octree::~Octree() {
    clear();
}

int Octree::getDepth() {
    if(!nodes.empty()) {
        return nodes.at(0)->getDepth();
    }

    return level;
}

void Octree::clear() {
    objects.clear();

    if(!nodes.empty()) {
        for(unsigned int i = 0; i < nodes.size(); i++) {
            nodes.at(i)->clear();
            delete nodes.at(i);
        }
    }

    nodes.clear();
}

void Octree::split() {
    float hs = this->halfSize *= 0.5f;
    float x = this->position.x;
    float y = this->position.y;
    float z = this->position.z;

    //multi-dimmensional tree order
    nodes.push_back(new Octree(level + 1, glm::vec3(x-hs, y+hs, z-hs), hs));
    nodes.push_back(new Octree(level + 1, glm::vec3(x+hs, y+hs, z-hs), hs));
    nodes.push_back(new Octree(level + 1, glm::vec3(x-hs, y+hs, z+hs), hs));
    nodes.push_back(new Octree(level + 1, glm::vec3(x+hs, y+hs, z+hs), hs));

    nodes.push_back(new Octree(level + 1, glm::vec3(x-hs, y-hs, z-hs), hs));
    nodes.push_back(new Octree(level + 1, glm::vec3(x+hs, y-hs, z-hs), hs));
    nodes.push_back(new Octree(level + 1, glm::vec3(x-hs, y-hs, z+hs), hs));
    nodes.push_back(new Octree(level + 1, glm::vec3(x+hs, y-hs, z+hs), hs));
}

int Octree::getIndex(glm::vec3 position,float halfSize) {
    // returns the index of the child node the aabb fits completely in.
    if(halfSize == FLT_MAX) {
        return -1;
    }

    int index = -1;
    float x = position.x;
    float y = position.y;
    float z = position.z;

    float xB = this->position.x;
    float yB = this->position.y;
    float zB = this->position.z;

    float hs = halfSize;

    if(y - hs > yB) { //top
        if(z - hs > zB) { //back
            if(x - hs > xB) { //right
                index = 3;
            } else if(x + hs < xB) { //left
                index = 2;
            }
        } else if(z + hs < zB) { //front
            if(x - hs > xB) { //right
                index = 1;
            } else if(x + hs < xB) { //left
                index = 0;
            }
        }
    } else if(y + hs < yB) { //bottom
        if(z - hs > zB) { //back
            if(x - hs > xB) { //right
                index = 7;
            } else if(x + hs < xB) { //left
                index = 6;
            }
        } else if(z + hs < zB) { //front
            if(x - hs > xB) { //right
                index = 5;
            } else if(x + hs < xB) { //left
                index = 4;
            }
        }
    }

    return index;
}

void Octree::insert(GameObject* object) {
    if(!object->hasCollider()) {
        //top level
        objects.push_back(object);
        return;
    }

    if(!nodes.empty()) {
        SphereCollider* sphereCollider = (SphereCollider*) object->getCollider();
        int index = getIndex(sphereCollider->getParent()->getLocation(), sphereCollider->getRadius());

        if(index != -1) {
            // fits completely in one
            nodes.at(index)->insert(object);
            return;
        }
    }

    objects.push_back(object);

    if(objects.size() > MAX_OBJECTS && level < MAX_LEVEL) {
        subdivide();
    }
}

void Octree::subdivide() {
    if(nodes.empty()) {
        split();
    }

    unsigned i = 0;

    for(std::vector<GameObject*>::iterator iter = objects.begin(); iter != objects.end(); iter++) {
        GameObject* object = (*(iter));

        if(!object->hasCollider()) {
            i++;
        } else {
            SphereCollider* sphereCollider = (SphereCollider*) object->getCollider();
            int index = getIndex(sphereCollider->getParent()->getLocation(), sphereCollider->getRadius());

            if(index != -1) {
                nodes.at(index)->insert(object);
                objects.erase(iter);
                iter--;
            }
        }
    }
}

void Octree::detectCollisions() {
    for(unsigned i = 0; i < objects.size(); i++) {
        GameObject* obj1 = objects.at(i);

        if(!obj1->hasCollider()) {
            continue;
        }

        for(unsigned j = i + 1; j < objects.size(); j++) {
            CollisionDetector::collide(obj1, objects.at(j));
        }

        if(!nodes.empty()) {
            for(unsigned child = 0; child < nodes.size(); child++) {
                nodes.at(child)->detectCollisions(obj1);
            }
        }
    }

    if(!nodes.empty()) {
        for(unsigned child = 0; child < nodes.size(); child++) {
            nodes.at(child)->detectCollisions();
        }
    }
}

void Octree::detectCollisions(GameObject* object) {
    if(!object->hasCollider()) {
        return;
    }

    for(unsigned i = 0; i < objects.size(); i++) {
        CollisionDetector::collide(object, objects.at(i));
    }

    if(!nodes.empty()) {
        for(unsigned child = 0; child < nodes.size(); ++child) {
            nodes.at(child)->detectCollisions(object);
        }
    }
}

void Octree::intersectsRay(CollisionRay* ray, Asteroid*& foundAsteroid, float furthest) {
    float currentPos;

    for(std::vector<GameObject*>::iterator it = objects.begin(); it != objects.end(); ++it) {
        if(Asteroid* asteroid = dynamic_cast<Asteroid*>(*it)) {
            if(((SphereCollider*) asteroid->getCollider())->rayIntersection(ray, currentPos)) {

                if(currentPos < furthest) { //little logic to get the nearest asteroid that catched the ray.
                    furthest = currentPos;
                    foundAsteroid = asteroid;
                }
            }
        }
    }

    if(!nodes.empty()) {
        for(unsigned int i = 0; i < nodes.size(); ++i) {
            nodes.at(i)->intersectsRay(ray, foundAsteroid, furthest);
        }
    }
}

void Octree::print(std::string pre) {
    std::cout << pre << level << "\t - Objects: " << objects.size() << "\t";

    for(unsigned int i = 0; i < objects.size(); i++) {
        std::cout << objects[i]->getName() << ", ";
    }

    std::cout << std::endl;

    for(unsigned int i = 0; i < nodes.size(); i++) {
        if(nodes[i]) {
            nodes[i]-> print(pre+"  ");
        }
    }
}
